-----------------------------------------------------------------------
--  File:       Controller.adb
-- Description: 
-- Controllers is the main logic engine of the system. It handles
-- - Sensor events: determine current train position in order for the system to take appropriate actions.
-- - User inputs: Speed, Direction of travel, Turning Direction, Halting.
-- In order to use Controllers, trains position needs to be initialized, also Debug Windows need to be passed in for extra info
-- Other operations are as described below

-- Revision

-- Todo:
-- (DONE) Train doesn't check whether the turn out is in position or not --> away status in Turnout_controller from maria
-- (DONE) IMplemented but Get_Status doesnt seem to work properly
-- (DONE) Hearbeat Implemented
-- (Done) Ensure Crossing sequence in Actual Tick is correct  (i.e. Fwd = (S-N) Bwd = (N-S))
-- (Done) Test out Controller/Crossing
-- Throw exception Init_Trains
-- (Done) Apply Auto Reverse
-- (Done) Check for crossing and zone (Auto-resume)
-- (done) Ignore Fast command when waiting
-- (Done) Add retry turnout 
-- Authors:     Ha Le, Maria
-----------------------------------------------------------------------

with Projdefs;
with Ada.Text_Io; --*
use Projdefs;
with Raildefs;
use Raildefs;
with Dac_Controller;
use Dac_Controller;
with Blocks;
with Unsigned_Types;
use Unsigned_Types;
with Topolog2;
use Topolog2;
with Turnout_Controller;
use Turnout_Controller;
with Crossing;
use Crossing;
with Zone;
use Zone;
with Swindows;
use Swindows;

package body Controllers is


--------------------------------------------------------------------------------
   ------- Definition -----
--------------------------------------------------------------------------------


   -- 3 Positions defining one Train
   -- Front and rear represent the magnet at front and rear of the actual train
   -- Tail Release is a software only Position used to release blocks at rear end
   type Position_Ref is
         (Front,
          Rear,
          Tail_Release);
   type Whole_Train_Position is array (Position_Ref) of Topolog2.Train_Position;
   Trains_Pos : array (Train_Id) of Whole_Train_Position;
   
   -- DEVIATION FROM SRS
   -- Saving previous sensor state, resolving weird problem with simulator when multiple sensor events fired for one sensor
   Prev_S_Evt_State: array (Train_Id,Position_Ref) of Sensor_Bit := (others =>(others=>Off));
    
    
   --DAC Controller
   --In order to resume to orginial (user-set) speed after a software halt
   Dac_Resume_Value : array (Train_Id) of Unsigned_8;

    
   --Blocks owned by train, used in releasing expired blocks
   Blocks_Owned   : array (Train_Id, Raildefs.Block_Idx) of Boolean := (others => (others => False));
   
  
   -- Used to ensure all sensor event has passed (skidding) prior to Actually Turn Around
   DELAY_TURNING_MAX : Natural := 1;
   Delay_Turning_Around : array (Train_Id) of Natural:= (others => DELAY_TURNING_MAX);
    
   --Debuging Window
   type Line_Array is array (Train_Id) of Natural;
   Line_Number   : Line_Array  := (others => 1); 
   Window_Length : Natural     := 13;
   Fg : array (Train_Id) of Swindows.Color := (others => Swindows.White);
   Debug_W       : Debug_Array;
   --Block_Debug_W : Swindows.Window; 
    
    
    --CAB Control
	-- Timout deprecated
   --SLOW_DOWN_TIMEOUT: Natural := 3; 
   -- Will resume speed when the sensor stored in Slow_Down_Flag is reached, by then, travel slow to ensure safety
   Slow_Down_Flag:  array (Train_Id) of Sensor_Idx:= (others => No_Sensor);
   
   --Init Delay
   Tick_Delay    : Natural     := 2;
   Debug_W_Delay : Natural     := 10;
   -- Wait for rear to reach pass the first sensor before starting Tail R
   INIT_DELAY_TAIL_RELEASE: constant Natural := 2;  
   Tail_Release_Delay : array (Train_Id) of Natural := (others => INIT_DELAY_TAIL_RELEASE);
    
   --Feature
   Empty_Feature : Feature_Descriptor_Extended := Feature_Descriptor_Extended'(Kind => Plain_Track, Block_Num => No_Block, Complete => True,Block_Next => No_Block);
    

   --Support Procedure 
   procedure Write_Debug(
		Window_Id: Train_Id;
		Message: String
		) is
	begin
		--Line 1 reserved for block and sensor
		Put_Line(Debug_W(Window_Id), Message, Line_Number(Window_Id) +1, Foreground=>Fg(Window_Id));
		Line_Number(Window_Id) := (Line_Number(Window_Id) mod Window_Length) + 1;
		if Line_Number(Window_Id) = 1 then
			if Fg(Window_Id) = White then Fg(Window_Id) := Yellow; else Fg(Window_Id) := White; end if;
		end if;
    end Write_Debug;

	
	-- Type of operation for used by Buffer
   type Op_Type is
         (Forward,
          Backward,
          Left,
          Right,
          Centre,
          Speed,
          Fast,
          Slow,
          Halt,
          Sensor_Event,
          Tick,
          Manned,
          Unmanned);
   subtype Speed_Ops is Op_Type range Fast..Slow;

   subtype Mode_Ops is Op_Type range Manned..Unmanned;
   type Train_Mode_Manned is array (Train_Id) of Boolean;
   T_Mode_Manned : Train_Mode_Manned := (True, True, True, True);

   --Parameters used in each Operation
   --Note that there are separate workers thread for each train so there's no need to specified Which Train
   type Item_Type (Req: Op_Type := Halt) is
   record
      case Req is
         when Forward=>
            null;
         when Backward=>
            null;
         when Left=>
            L_Id: Train_Id:=1;
         when Right=>
            R_Id:  Train_Id:=1;
         when Centre=>
            C_Id: Train_Id:=1;
        when Speed =>
            Speed_Value: Unsigned_8 := 0;
         when Fast =>
            null;
         when Slow =>
            null;
         when Halt =>
            null;
         when Sensor_Event =>
            S_Id: Raildefs.Sensor_Id;
            S_State: Raildefs.Sensor_Bit;
         when Tick =>
            null;
         when Manned =>
            null;
         when Unmanned =>
            null;
      end case;
   end record;

 
   
--------------------------------------------------------------------------------
   ------- Initialization -----
--------------------------------------------------------------------------------
   procedure Init_W_Int (
         Debug_Win : in     Debug_Array) is
   begin
      Debug_W:= Debug_Win;
   end Init_W_Int;

   procedure Init_Trains (
         Init_Pos : in     Train_Init_Position) is
      --TempID  : Sensor_Idx;
      Success : Boolean;
      --Zone    : Zone_Id;
   begin
      for I in Init_Pos'range loop
         -- all cab null
         Dac_Controller.Set_Voltage((Cab_Type(I)),0);
         if(Init_Pos(I) /= 0) then
            Trains_Pos(I)(Front) := Initialise_After(Init_Pos(I));
            Trains_Pos(I)(Rear) := Initialise_After(Init_Pos(I));
            Trains_Pos(I)(Tail_Release) := Trains_Pos(I)(Rear);
			
            Ada.Text_Io.Put_Line("Block Ac"&Trains_Pos(I)(Front).Block_Num'Img); --*
            Blocks.Acquire_Block(Success,I,Trains_Pos(I)(Front).Block_Num);
            if(Success) then
               Blocks.Set_Block(Success,I,Trains_Pos(I)(Front).Block_Num,
                  Cab_Type(I),Trains_Pos(I)(Front). Polr);
               Blocks_Owned(I,Trains_Pos(I)(Front).Block_Num) := True;
               --Write_Debug(Train_Id(I),"Block Init"&Trains_Pos(I)(Front).Block_Num'Img);
            else
               -- Throw Exception Here -- 2 Trains on same block!
               null;
            end if;
         end if;
      end loop;

   end Init_Trains;

    
    --------------------------------------------------------------------------------
    ------- Buffer Object -----
    --------------------------------------------------------------------------------
    N : constant := 16;
    type Index_Type is mod N;
    type Item_Array is array (Index_Type) of Item_Type;
    
    protected type Buffer_Type is
        procedure Start (
                Event : in     Item_Type);
    
        entry Wait_Start (
                Event    :    out Item_Type;
                Over_Run :    out Boolean);
    private
        Items: Item_Array;
        Iin, Jout : Index_Type := 0;
        Count : Integer := 0;
        Item_Available,
            Too_Fast : Boolean := False;
    end Buffer_Type;
    ---
    task type Thread_Type (Me : Train_Id);
    
    Worker1 : Thread_Type (1);
    Worker2 : Thread_Type (2);
    Worker3 : Thread_Type (3);
    ------------
    Buffers : array (Train_Id) of Buffer_Type;
    
        
    protected body Buffer_Type is
        procedure Start (
                Event : in     Item_Type) is
        begin
            if( Count >= N) then
                Too_Fast := True;
            end if;
    
            -- ignore old values
            Items (Iin) := Event;
            Iin := Iin + 1;
            Count := Count + 1;
            Item_Available := True;
        end Start;
    
        entry Wait_Start (
                Event    :    out Item_Type;
                Over_Run :    out Boolean) when Item_Available is
        begin
            Event := Items(Jout);
            Jout := Jout + 1;
            Count := Count - 1;
            Over_Run := Too_Fast;
            if(Count = 0) then
                Item_Available := False;
            end if;
            Too_Fast := False;
        end Wait_Start;
    end Buffer_Type;   
    
--------------------------------------------------------------------------------
   ------- Operations -----
--------------------------------------------------------------------------------
	-- All functions here only place the appropriate request into the buffer.
	-- The worker thread will handle actual functionalities

   procedure Start_Train_Forward (
         T_Id : in     Train_Id) is
   begin
      Buffers(T_Id).Start(Item_Type'(Req => Forward));
   end Start_Train_Forward;

   procedure Start_Train_Backward (
         T_Id : in     Train_Id) is
   begin
      Buffers(T_Id).Start(Item_Type'(Req => Backward));
   end Start_Train_Backward;


   procedure Start_Train_Direction_Left (
         T_Id : in     Train_Id) is
   begin
      Buffers(T_Id).Start(Item_Type'(Left,T_Id));
   end Start_Train_Direction_Left;


   procedure Start_Train_Direction_Right (
         T_Id : in     Train_Id) is
   begin
      Buffers(T_Id).Start(Item_Type'(Right,T_Id));
   end Start_Train_Direction_Right;



   procedure Start_Train_Direction_Centre (
         T_Id : in     Train_Id) is
   begin
      Buffers(T_Id).Start(Item_Type'(Centre,T_Id));

   end Start_Train_Direction_Centre;

    procedure Start_Train_Speed (
            T_Id : in     Train_Id;
            Speed_Value   : in Unsigned_8) is
        
    begin
        Buffers(T_Id).Start(Item_Type'(Req => Speed,Speed_Value=>Speed_Value));
    end Start_Train_Speed ;

   procedure Start_Fast (
         T_Id : in     Train_Id) is
   begin
      Buffers(T_Id).Start(Item_Type'(Req => Fast));
   end Start_Fast;

   procedure Start_Slow (
         T_Id : in     Train_Id) is
   begin
      Buffers(T_Id).Start(Item_Type'(Req => Slow));
   end Start_Slow;

   procedure Start_Halt (
         T_Id : in     Train_Id) is
   begin
      Buffers(T_Id).Start(Item_Type'(Req => Halt));
      Line_Number(T_Id):= (Line_Number(T_Id) mod Window_Length) +1;
   end Start_Halt;

   
   procedure Start_Sensor_Event (
         S_Id    : in     Raildefs.Sensor_Id;
         S_State : in     Raildefs.Sensor_Bit) is
   begin
		for I in Trains_Pos'range loop
			Buffers(I).Start(Item_Type'(Req => Sensor_Event,S_Id=>S_Id,S_State=>S_State));
		end loop;
   end Start_Sensor_Event;

   
   procedure Start_Tick is
   begin
      for I in Trains_Pos'range loop
         Buffers(I).Start(Item_Type'(Req => Tick));

      end loop;

   end Start_Tick;

   procedure Start_Manned (
         T_Id : in     Train_Id) is
   begin
      Buffers(T_Id).Start(Item_Type'(Req => Manned));


   end Start_Manned;




   procedure Start_Unmanned (
         T_Id : in     Train_Id) is
   begin
      Buffers(T_Id).Start(Item_Type'(Req => Unmanned));


   end Start_Unmanned;
   

   
--------------------------------------------------------------------------------
   ------- Threads -----
--------------------------------------------------------------------------------

	------- Type Definition -----
   task body Thread_Type is

	  -- Queue of un-acquired resources, to be retried later on
	  Feature_Queue : array (Train_Id,Feature_Kind) of Feature_Descriptor_Extended := (others => (others=>Empty_Feature));	
	  -- Flags used internally my different procedure to determine whether next block is of opposite polr
	  Polr_Opposite  : array (Train_Id) of Boolean      := (others => False);
	  -- Flags used to keep track of moving/stopping status
	  Stopping      : array (Train_Id) of Boolean := (others => False);
	  -- Whether the stop is by user or by software
      Manually_Halted:   array (Train_Id) of Boolean := (others => False);
     
    
        
        --------------------------------------------------------------------------------
        ------- Interface Functions -----
        --------------------------------------------------------------------------------   
        -- Halt the train and wait a bit for skidding pass
        -- Turn the train around logically
        -- Reverse polarity of all blocks
        procedure Actual_Turn_Around (
            Fail_Metric      : in   out Natural);

        -- Call Turnaround if current direction is backward
        procedure Actual_Forward ;
        -- Call Turnaround if current direction is forward
        procedure Actual_Backward;
        
        -- Called by both interface module or internally
        -- Stop the train and set flags
        -- Stopping Flag True
        -- Manually Halted Flag depends on software or user stop command     
        procedure Actual_Halt (
                Manual : Boolean := True);
     
        -- 
        procedure Actual_Centre (
                T_Id           : in     Raildefs.Train_Id;
                Position_Front : in     Topolog2.Train_Position);
    
        procedure Actual_Left (
                T_Id           : in     Raildefs.Train_Id;
                Position_Front : in     Topolog2.Train_Position);
        procedure Actual_Right (
                T_Id           : in     Raildefs.Train_Id;
                Position_Front : in     Topolog2.Train_Position);
          
        procedure Actual_Mode (
                Mode : in     Mode_Ops);
    
        -- Advance the train position logically (if sensor event is valid)
        -- Perform necessary check to acquire resources or store them in Feature_Queue for later recheck
        -- If needed, release used resource
        procedure Actual_Sensor_Event (
                S_Id      : in     Raildefs.Sensor_Id;
                S_State   : in     Raildefs.Sensor_Bit
                ); 

        -- Advance the train position logically (if sensor event is valid)
        -- Perform necessary check to acquire resources or store them in Feature_Queue for later recheck
        -- If needed, release used resource    
        procedure Actual_Tick; 
        
        -- Check if the train has been stopped logically, if yes ignore the command as it's not safe
        -- If not, increase or decrease speed and set Stopping and Manual flag 
        procedure Actual_Speed_Op (
            Op_Type : in     Speed_Ops);

        -- Check if the train has been stopped logically, if yes ignore the command as it's not safe
        -- If not, set the speed and flags accordingly
        procedure  Actual_Speed_Set(
                Speed_Value : in Unsigned_8);
    
    
        --------------------------------------------------------------------------------
        ------- Feature Functions ----- 
        --      Handle features-related functionality is detecting, acquiring, rechecking 
        --      queuing of features
        --------------------------------------------------------------------------------
        -- Convert_To_X_Feature 
        -- Convert from Topolog2 Feature_Descriptor to Feature_Descriptor_Extended
        -- With added info/functionalities needed by other modules
        function Convert_To_X_Feature (
                Feature : in     Feature_Descriptor)
            return Feature_Descriptor_Extended ;
    
        -- Resume the train topologically, checking for required turnout setting and 
        procedure Train_Topo_Resume (
                T_End: Position_Ref := Front);
    
        -- Called at end of a safety check (on Sensor event or Tick)
        -- Make the train resume moving if has been stopped waiting for resources
        -- Reset flags 
        procedure End_Safe_Check (
                Resume_Success : Boolean);
    
        -- Called to perform all check: Block, Turnout, Crossing etc. and update the queue
        -- Does NOT call Feature Check at end, assuming next tick will do
        procedure Update_Feature_Queue;
    
        -- Check the Feature to see if resource available, 
        -- if not increase fail metric
        -- if yes set the Feature to Empty_Feature
        procedure Feature_Check (
                Fail_Metric : in out Natural;
           Cur_Feature: in out Feature_Descriptor_Extended
              );
        
        -- Call Feature Check then if fail halt the train
        -- Callled if immediate halt is neccessary (one feature failed is enough)
        procedure Feature_Check_Halt(
            Feature: in out Feature_Descriptor_Extended);
    
        -- Check if requested block is owned by the train, 
        -- And further check if the train (Front,Rear) is on that block or not
        -- If pass both condition release the block
        -- Report failure if needed
        procedure Release_Block_Extended(
                Success : out Boolean;
                Block: Block_Idx);
                
        --------------------------------------------------------------------------------
        ------- Turnout Functions -----
        --------------------------------------------------------------------------------            
         -- Check if the turnout  is converging
        function Check_Converging (
            Turnout : Turnout_Id;
            Polr    : Polarity_Type)
        return Boolean;
        
        -- Acquire and set the Turnout
        -- Increase Fail Metric if failed
        procedure Turnout_Block_Acquire_Set (
                Fail_Metric          :  in  out Natural;
                Block            : in  out   Block_Idx;
                Polr              : in      Polarity_Type;
                Which_Turnout    : in     Turnout_Id;
                Required_Setting : in   Turnout_Pos
                ); 
    
        -- Set the turn out and its appropriate block
        procedure Turnout_Block_Set_Check (
                Fail_Metric      : in   out Natural;
                Block            :        Block_Idx;
                Polr             :        Polarity_Type;
                Turnout          :        Turnout_Id;
                Converging       :        Boolean;
                Required_Setting :        Turnout_Pos
                );
    
        -- Get information about the tunrout
        -- Used if information from the queue might be outdated
        -- (i.e. user might decided to turn when Check Entering already completed
        procedure Get_Turning_Info (
                Fail_Metric: in out Natural;
                Next_Block :    out Block_Idx;
                Cur_Polr   :        Polarity_Type;
                Turnout    :        Turnout_Id;
                Converging :        Boolean);
    
        --------------------------------------------------------------------------------
        ------- Sensor Events -----
        --------------------------------------------------------------------------------   
        -- Advance pos with added support for adding turnout information if required
        -- Check if Sensor State is valid (i.e. an On state followed by another On is invalid)
        -- ignore and notify debug windows if such abnormality occurs
        procedure Advance_Pos_Extended(
               Which_End: Position_Ref;
                State: Sensor_Bit
                );
    
        -- Perform the main task of sensor event
        -- Advance Pos
        -- Check for coming Feature at Front
        -- Check for releasing feature at rear
        procedure Sensor_Event_Full(
                S_Id      : in     Raildefs.Sensor_Id;
                S_State   : in     Raildefs.Sensor_Bit;
                Which_End : in     Position_Ref
                );
        --------------------------------------------------------------------------------
        ------- Sensor ON Front event Checks-----
        --------------------------------------------------------------------------------      
        -- Check, attemp to acquire, if fail record the crossing into the return value
        -- else return Empty_Feature
        function Check_Entering_Crossing_Extended (
                Pos      : in     Train_Position)
                return Feature_Descriptor_Extended;
        
        -- Check, attemp to acquire, if fail record the zone into the return value
        -- else return Empty_Feature
        function  Check_Entering_Zone_Extended (
                Pos      : in     Train_Position
                )
                return Feature_Descriptor_Extended;
    
        -- Check if the train is moving toward a "Dangerous" sensor, as defined in ProjDefs
        -- if yes set the speed to a defined SLOW value
        procedure Check_Go_Slow;
    
        -- 
        function  Check_Entering_Dead_Track (
                Pos     :  Train_Position  ) return   Feature_Descriptor_Extended;
    
        procedure Check_Speed_Back;
    
        --------------------------------------------------------------------------------
        ------- Sensors OFF Rear Event Checks -----
        --------------------------------------------------------------------------------    
      
        procedure  Check_Leaving_Turnout_Extended (
                Pos        : in     Train_Position); 
        procedure  Check_Leaving_Crossing_Extended (
                Pos        : in     Train_Position);
        procedure  Check_Leaving_Zone_Extended (
            Pos        : in     Train_Position);
        --------------------------------------------------------------------------------
        ------- Blocks Functions -----
        --------------------------------------------------------------------------------            
        -- Acquire and set the block
        -- Increase Fail Metric if failed
        procedure Block_Acquire_Set (
                Fail_Metric:   in out Natural;
                Block    : in     Block_Idx;
                Polr     : in     Polarity_Type;
                Opp_Polr : in  Boolean);

    --------------------------------------------------------------------------------
    ------- Functions and Procedure Body-----
    -------------------------------------------------------------------------------- 
                   
    procedure Actual_Halt (
            Manual : Boolean := True)
            is
        Temp_Dac : Unsigned_8;
        
      begin
         
        Dac_Controller.Get_Voltage(Cab_Type(Me),Temp_Dac);
        -- Don't get the slow-downed value
        if(Temp_Dac /= 0 and Slow_Down_Flag(Me) = No_Sensor) then
            Dac_Resume_Value(Me) := Temp_Dac;  
            Write_Debug(Me,"Set DacRe in Halt");  
        end if; 
        
        if(Manual) then
            Dac_Resume_Value(Me) := 0;
            Manually_Halted(Me) := True;
        else
            --Software
            Slow_Down_Flag(Me) := Trains_Pos(Me)(Front).Next_Expected;
        end if; 
         Dac_Controller.Set_Voltage(Raildefs.Dac_Id(Me), 0);
         Stopping(Me) := True;
         Write_Debug(Me,"H! "&Manual'Img&Dac_Resume_Value(Me)'Img);--*
      end Actual_Halt;
    

    procedure  Actual_Speed_Set(
            Speed_Value : in Unsigned_8) is
        
    begin
        
        if Stopping(Me) = False or Manually_Halted(Me) = True then
            Write_Debug(Me,"Speed Set"&Speed_Value'Img);  
            Dac_Controller.Set_Voltage(Raildefs.Dac_Id(Me), Speed_Value);
            --Slow_Down_Flag(Me) := No_Sensor;
            if(Speed_Value > 0) then
                if(Stopping(Me) = True) then
                    Stopping(Me) := False;
                    Manually_Halted(Me) := False;
                    -- TODO Resume here
                end if;
            else
                --Actual_Halt(Manual=>True);  
                Stopping(Me) :=  True;
            end if;    
        end if; 
--        Dac_Resume_Value(Me) := Speed_Value;     
    end Actual_Speed_Set;
    
    
      --Speed controller
      procedure Actual_Speed_Op (
            Op_Type : in     Speed_Ops) is
         Current_Value : Unsigned_8;
         Updated_Value : Unsigned_8;
      begin
         --Ada.Text_Io.Put_Line("Speed"&Stopping(Me)'Img&Manually_Halted(Me)'Img);--*
         if(Stopping(Me) = False or Manually_Halted(Me) = True ) then -- is not waiting	
            Dac_Controller.Get_Voltage(Raildefs.Dac_Id(Me),Current_Value);
            case Op_Type is
               when Fast =>
                  Updated_Value := Current_Value + ProjDefs.SPEED_STEP;
                  if( Updated_Value < Current_Value) then -- overflow
                     Updated_Value := Unsigned_8'Last;
                  end if;
				  if(Stopping(Me) = True) then
						Stopping(Me) := False;
                    Manually_Halted(Me) := False;
						-- TODO Resume here
				  end if;		

               when Slow =>
                  Updated_Value := Current_Value - ProjDefs.SPEED_STEP;
                  if( Updated_Value > Current_Value) then -- overflow
                     Updated_Value := Unsigned_8'First;
                     Stopping(Me) :=  True;
                  end if;

            end case;

            Dac_Controller.Set_Voltage(Raildefs.Dac_Id(Me), Updated_Value);
--            Slow_Down_Flag(Me) := No_Sensor;
         end if;
      end Actual_Speed_Op;

    

    procedure Train_Topo_Resume (
            T_End: Position_Ref := Front)
            is
        Need_Turnout_Info : Boolean;
         Turnout_Need      : Turnout_Id;
        Pos : Turnout_Pos;
    begin
        
        --for T_End in Position_Ref loop
           -- Trains_Pos(Me)(T_End).Substate := Norm;
            Check_Turnout_Info_Need(Trains_Pos(Me)(T_End),Need_Turnout_Info,Turnout_Need);
            if(Need_Turnout_Info) then
                Pos := Get_Position(Turnout_Need);
            else
                Pos := Middle;
            end if;
            Resume(Trains_Pos(Me)(T_End),Pos);      
            Ada.Text_Io.Put_Line("After"&T_End'Img&"Resume S"&Trains_Pos(Me)(T_End).Next_Expected'Img);--*
        --end loop;
    exception
        when Illegal_Position =>
            Write_Debug(Me,"Train Topo Failed Ill Pos");
            null;
        when Missing_Turnout_Info =>
            null;
            Write_Debug(Me,"Train Topo Failed Tn Info");
        
    end Train_Topo_Resume;


    
    procedure Feature_Check (
            Fail_Metric : in out Natural;
       Cur_Feature: in out Feature_Descriptor_Extended
          )            is
             Resume_Success: Boolean;
    begin
        Write_Debug(Me,"FC K"&Cur_Feature.Kind'Img);
        if(Cur_Feature.Block_Next /= No_Block) then
            Block_Acquire_Set(Fail_Metric,Cur_Feature.Block_Next,Trains_Pos(Me)(Front).Polr,Polr_Opposite(Me));
        end if;
        
        case Cur_Feature.Kind is 
           when Plain_Track =>
              -- Acquire Block 
              -- polr opposite always false since plain track
              Block_Acquire_Set(Fail_Metric, Cur_Feature.Block_Num,Trains_Pos(Me)(Front).Polr,Polr_Opposite(Me));
           when Sensor =>
              Block_Acquire_Set(Fail_Metric, Cur_Feature.Block_Num,Trains_Pos(Me)(Front).Polr,Polr_Opposite(Me));
              if(Cur_Feature.Block2 /= No_Block) then
                 Block_Acquire_Set(Fail_Metric, Cur_Feature.Block2,Trains_Pos(Me)(Front).Polr,Polr_Opposite(Me));
              end if;
           when Turnout => 
              Turnout_Block_Acquire_Set(Fail_Metric,Cur_Feature.Block_Num,Trains_Pos(Me)(Front).Polr,Cur_Feature.Tid,Cur_Feature.Setting);
               if (Cur_Feature.Tid2 /= No_Turnout) then
                 Turnout_Block_Acquire_Set(Fail_Metric,Cur_Feature.Block_Tn2,Trains_Pos(Me)(Front).Polr,Cur_Feature.Tid2,Cur_Feature.Setting2);
              end if; 
           when Boundary_Zone =>
              Acquire_Zone(Resume_Success,Me,Cur_Feature.Zid);
             if(Resume_Success = False) then 
                Fail_Metric := Fail_Metric+1;  
                Write_Debug(Me,"Fail!! Z"&Cur_Feature.Zid'Img); 
             else 
                
                Block_Acquire_Set(Fail_Metric, 
                    Topolog2.Sensor_Block_Numbers(Trains_Pos(Me)(Front).Polr,Cur_Feature.Zid),
                    Trains_Pos(Me)(Front).Polr,False);
             end if;
                
           when Crossing_Track => 
              Acquire_Crossing(Resume_Success,Me,Cur_Feature.Kid);
              if(Resume_Success = False) then 
                Fail_Metric := Fail_Metric+1;  
                Write_Debug(Me,"Fail!! C"&Cur_Feature.Kid'Img); 
              end if;
                
              Acquire_Crossing(Resume_Success,Me,Cur_Feature.Kid2);
              if(Resume_Success = False) then  
                Fail_Metric := Fail_Metric+1;  
                Write_Debug(Me,"Fail!! C"&Cur_Feature.Kid2'Img); 
              end if;
                
           when Dead_Track =>
              Actual_Turn_Around(Fail_Metric);
        end case;

    end Feature_Check;
    
    procedure Feature_Check_Halt(
            Feature: in out Feature_Descriptor_Extended) is
        Fail_Metric : Natural;
    begin
        Fail_Metric := 0;
        Feature_Check(Fail_Metric, Feature);
        if (Fail_Metric /= 0) then
            Write_Debug(Me,"F Ck Failed");
            Actual_Halt(Manual => False);
        end if;
    end Feature_Check_Halt;
            

    procedure End_Safe_Check (
            Resume_Success : Boolean) is
        Cur_Feature : Feature_Descriptor_Extended;
       Temp_Dac          : Unsigned_8;
       Fail_Metric : Natural := 0;
      begin
         if(Resume_Success) then
            -- Extra quick check prior to resume
            --Ada.Text_Io.Put_Line("Checking Resume Now");    
            for F_Kind in Topolog2.Feature_Kind loop
               Feature_Queue(Me,F_Kind) := Empty_Feature;
            end loop;
            
            
            --Write_Debug(Me,"End Sf"&Stopping(Me)'Img&Manually_Halted(Me)'Img);
            
            
            -- if resume needed
            -- waiting and not manuall halted
            if Stopping(Me) = True and Manually_Halted(Me) = False then
              
                if  Slow_Down_Flag(Me) /= No_Sensor then
                    --Slow_Down_Flag(Me):= Slow_Down_Flag(Me)-1;
                    if Me = 1 then -- train w load
                        Temp_Dac :=  SPEED_SLOW_T1;
                    else  
                        Temp_Dac :=  SPEED_SLOW;
                    end if;        
                    Write_Debug(Me,"Dac_Resume"&Dac_Resume_Value(Me)'Img); 
                else 
                   
                    if(Dac_Resume_Value(Me)/= 0) then -- avoid initial zero values
                        Temp_Dac :=    Dac_Resume_Value(Me); 
                    end if;
                    
                end if;
                
                Cur_Feature := Convert_To_X_Feature(Topolog2.Feature_At(Trains_Pos(Me)(Front)));
                Write_Debug(Me,"F@A_Re"&Cur_Feature.Kind'Img);
                Feature_Check(Fail_Metric,Cur_Feature);
                
                -- Resume moving
                Train_Topo_Resume(Front);
                Stopping(Me) := False;
                Write_Debug(Me,"Resume From Stop"&Temp_Dac'Img);
                Ada.Text_Io.Put_Line("Resume From Stop"); --*
                Dac_Controller.Set_Voltage(Dac_Id(Me),Temp_Dac);
                                   
             end if; 
   
         else
            Dac_Controller.Get_Voltage(Raildefs.Dac_Id(Me),Temp_Dac);
            if(Temp_Dac /= 0 and Slow_Down_Flag(Me) = No_Sensor ) then
               Dac_Resume_Value(Me) := Temp_Dac;
               Write_Debug(Me,"Set DacRe EndC");
            end if;
            Actual_Halt(Manual=>False); --dangerous -- stop waiting
         Write_Debug(Me,"Halt"); 
         end if;

      end End_Safe_Check;
        
      procedure Release_Block_Extended(
            Success : out Boolean;
            Block: Block_Idx) is
        
    begin
        Success := True;
        for T_Ref in Front..Rear loop
            Ada.Text_Io.Put_Line("Attempt to R BLk"&Block'Img&" C"&Trains_Pos(Me)(T_Ref).Block_Num'Img);--*
            if Block = Trains_Pos(Me)(T_Ref).Block_Num then
                Success := False;
            end if;     
        end loop;
        if(Success) then
            Blocks.Set_Block(Success,Me,Block,0,Fwd); 
            if Success then
                Blocks.Release_Block(Success,Me,Block);
                Blocks_Owned(Me,Block) := False;
                Write_Debug(Me, "S! Rl Blk "&Block'Img);
            else
                Write_Debug(Me, "F! Rl Blk "&Block'Img);      
                null;
            end if;    
        end if;
        
    end Release_Block_Extended;

      procedure Actual_Turn_Around (
            Fail_Metric : in out Natural)
            is
         Position_Temp    : Topolog2.Train_Position;
         New_Tail_Sensor  : Sensor_Idx;
         Success          : Boolean;
         New_Front_Sensor : Sensor_Idx;
         Need_Setting     : Boolean;
         For_Turnout      : Turnout_Id;
        Temp_Next_Sensor  : Sensor_Idx;
         Next_Block     : Block_Idx := No_Block;
      begin

        --Clear Flag 
        --Turning_Around(Me) := False; 
      if(Stopping(Me) = True) then
        if(Delay_Turning_Around(Me) = 0) then 
            Delay_Turning_Around(Me) := DELAY_TURNING_MAX;
            Ada.Text_Io.Put_Line("Turing Around Now");--*
            
            
            --Off Next Expecting Block if acquired already
            Next_Block := Topolog2.Sensor_Block_Numbers(Trains_Pos(Me)(Front).Polr, Trains_Pos(Me)(Front).Next_Expected);
            if(Next_Block /= Trains_Pos(Me)(Front).Block_Num) then
                Release_Block_Extended(Success,Next_Block);
                if(Success) then
                    Next_Block := ProjDefs.Chained_Turnout_Record(Trains_Pos(Me)(Front).Polr,Next_Block).Block1;
                    if( Next_Block /= No_Block) then
                        Release_Block_Extended(Success,Next_Block);
                    end if;
                    Next_Block := ProjDefs.Chained_Turnout_Record(Trains_Pos(Me)(Front).Polr,Next_Block).Block2;
                    if( Next_Block /= No_Block) then
                        Release_Block_Extended(Success,Next_Block);
                    end if;    
                end if;
            end if;                
           
            
            --Swaping btw Front and Rear 
            Position_Temp := Trains_Pos(Me)(Front);   
            Trains_Pos(Me)(Front) := Trains_Pos(Me)(Rear);
            Trains_Pos(Me)(Rear) := Position_Temp;
      
            -- Reinit Tail_Release
--            if (Me /= Tram_Id) then 
--                Trains_Pos(Me)(Tail_Release) := Position_Temp;
--                Advance_Pos_Extended(Tail_Release ,Opposite_Bit(Prev_S_Evt_State(Me,Tail_Release)));
--                Advance_Pos_Extended(Tail_Release ,Opposite_Bit(Prev_S_Evt_State(Me,Tail_Release)));
--            end if; 
            
--      
            -- Turn around
            for T_Ref  in Front..Rear loop
                Write_Debug(Me,"Tn Ard"); 
                Topolog2.Turn_Around(Trains_Pos(Me)(T_Ref)); 
                if T_Ref = Rear then
                    Train_Topo_Resume(Rear);
                end if;
                Write_Debug(Me,"Fn nex exp"&Trains_Pos(Me)(T_Ref).Next_Expected'Img&Trains_Pos(Me)(T_Ref).Block_Num'Img);
                
                Find_Next_Sensor_Expected(Trains_Pos(Me)(T_Ref),Temp_Next_Sensor,
                    Need_Setting,For_Turnout);
                
                Write_Debug(Me,"Check setting"&Need_Setting'Img&For_Turnout'Img);
                if(Need_Setting) then   
                    Temp_Next_Sensor := Next_Sensor_Expected(Trains_Pos(Me)(T_Ref),
                    Get_Position(For_Turnout));
                end if;
                Write_Debug(Me,"Tn Ard N"&Temp_Next_Sensor'Img&"A Ex"&Trains_Pos(Me)(T_Ref).Next_Expected'Img);
                
                Trains_Pos(Me)(T_Ref).Next_Expected := Temp_Next_Sensor;    
            end loop;
            
--            if Me = Tram_Id then
                Trains_Pos(Me)(Tail_Release):= Trains_Pos(Me)(Rear);
                Tail_Release_Delay(Me) := INIT_DELAY_TAIL_RELEASE;
--            end if;
            Write_Debug(Me,"Rev Blk Tn Ard");
            -- Release any pending blocks
            for B in Block_Idx  loop
                if (Blocks_Owned(Me,B)) then
                Blocks.Reverse_Block(Success,Me,B);
                end if;
            end loop;
            
            --Check
            Write_Debug(Me,"Chk Tn around");
           Check_Leaving_Turnout_Extended (
                   Trains_Pos(Me)(Tail_Release) ); 
            Check_Leaving_Crossing_Extended (
                    Trains_Pos(Me)(Tail_Release));
            Check_Leaving_Zone_Extended (
                Trains_Pos(Me)(Tail_Release));
            
            -- Release Pending Feature Queue
            --End_Safe_Check(True);
            for F_Kind in Topolog2.Feature_Kind loop
                Feature_Queue(Me,F_Kind) := Empty_Feature;
            end loop;
        else
            Delay_Turning_Around(Me) := Delay_Turning_Around(Me) - 1;
            Fail_Metric := Fail_Metric +1;
            Write_Debug(Me,"F! Tn Ard /=D"); 
        end if;
    else
        Actual_Halt(Manual => False);
        Fail_Metric := Fail_Metric +1;
        Write_Debug(Me,"F! Tn Ard St R");
    end if;  
        
        
      end Actual_Turn_Around;






      function Convert_To_X_Feature (
            Feature : in     Feature_Descriptor)
        return Feature_Descriptor_Extended is
      begin
         case Feature.Kind is
            when Plain_Track =>
               return Feature_Descriptor_Extended'(
                  Block_Next => No_Block,
                  Kind      => Plain_Track,
                  Block_Num => Feature.Block_Num,
                  Complete  => False);
            when Sensor =>
               return Feature_Descriptor_Extended'(
                  Block_Next => No_Block,
                  Kind      => Sensor,
                  Block_Num => Feature.Block_Num,
                  Complete  => False,
                  Sid       => Feature.Sid,
                  Block2    => Feature.Block2);
            when Turnout =>
               return Feature_Descriptor_Extended'(
                  Block_Next => No_Block,
                  Kind      => Turnout,
                  Block_Num => Feature.Block_Num,
                  Complete  => False,
                  Tid       => Feature.Tid,
                  Tid2      => No_Turnout,
                  Block_Tn2 => No_Block,
                  Setting   => Middle,
                  Setting2  => Middle,
                  Next_Tu   => Feature.Next_Tu
                  );
            when Boundary_Zone =>
               return Feature_Descriptor_Extended'(
                  Block_Next => No_Block,
                  Kind      => Boundary_Zone,
                  Block_Num => Feature.Block_Num,
                  Complete  => False,
                  Zid       => Feature.Zid
                  );
            when Crossing_Track =>
               return Feature_Descriptor_Extended'(
                  Block_Next => No_Block,
                  Kind      => Crossing_Track,
                  Block_Num => Feature.Block_Num,
                  Complete  => False,
                  Kid       => Feature.Kid,
                  Kid2       => Feature.Kid
                  );
            when others =>
               return Feature_Descriptor_Extended'(
                  Block_Next => No_Block,
                  Kind      => Plain_Track,
                  Block_Num => Feature.Block_Num,
                  Complete  => False
                  );
         end case;

      end Convert_To_X_Feature;

      function Check_Converging (
            Turnout : Turnout_Id;
            Polr    : Polarity_Type)
        return Boolean is
      begin
         if(Polr = Fwd) then
            if Turnout_Data(Turnout).Diverging_When_Normal then
               return False;
            else
               return True;
            end if;
         else
            if Turnout_Data(Turnout).Diverging_When_Normal then
               return True;
            else
               return False;
            end if;
         end if;
      end Check_Converging;
	
	  procedure Check_Go_Slow is
        Temp_Dac : Unsigned_8;
	  begin
         Ada.Text_Io.Put_Line("Ex "&Trains_Pos(Me)(Front).Next_Expected'Img&"Slow One"&Slow_Down_Flag(Me)'Img);--*
         -- Work only on moving train
		 if (Stopping(Me) = False and Is_Dangerous_Sensor(Trains_Pos(Me)(Front).Polr, Trains_Pos(Me)(Front).Next_Expected)) then
            Dac_Controller.Get_Voltage(Cab_Type(Me),Temp_Dac);
            if(Temp_Dac /= 0 and Slow_Down_Flag(Me) = No_Sensor) then 
                Dac_Resume_Value(Me) := Temp_Dac;
            end if;
            if Me = 1 then -- train w load
                Temp_Dac :=  SPEED_SLOW_T1;
            else
                Temp_Dac :=  SPEED_SLOW;
            end if;         
			Dac_Controller.Set_Voltage(Cab_Type(Me),Temp_Dac);
            Slow_Down_Flag(Me) := Trains_Pos(Me)(Front).Next_Expected;
			Write_Debug(Me,"SLOW DOWN");
            Ada.Text_Io.Put_Line("SLOW DOWN"); --*
            
		end if;
			
	end Check_Go_Slow;
    
    procedure Check_Speed_Back is
        
    begin
        if Trains_Pos(Me)(Front).Next_Expected = Slow_Down_Flag(Me) then
            --this happen in ON event only
            Slow_Down_Flag(Me) := No_Sensor;
        end if;
    end Check_Speed_Back; 
     
      procedure Get_Turning_Info (
            Fail_Metric: in out Natural;
            Next_Block :    out Block_Idx;
            Cur_Polr   :        Polarity_Type;
            Turnout    :        Turnout_Id;
            Converging :        Boolean) is
            Setting    :        Turnout_Pos := Middle;
      begin
         Write_Debug(Me,"T Info"&Turnout'Img&Converging'Img); 
         --Converging := Check_Converging(Turnout,Cur_Polr);
         --Ada.Text_Io.Put_Line("Turnout info"& Turnout'Img&Converging'Img);--*
         if(Converging) then
            Next_Block := Turnout_Data(Turnout).Block_Num;
            Write_Debug(Me,"TnInfo Blk"&Next_Block'Img);
            -- Setting here!!!!
         else
            -- figure out required setting
            -- Required_Setting := Middle; -- No need
            -- figure out what is the next block to acquire
            Setting := Get_Position(Turnout);

            Write_Debug(Me,"T Info"&Fail_Metric'Img);
            case Setting is
               when Straight =>
                  Next_Block :=    Turnout_Data(Turnout).Block_St;
               when Turned =>
                  Next_Block :=    Turnout_Data(Turnout).Block_Tu;
               when Middle =>
                  Next_Block :=  No_Block;
                  Fail_Metric:=Fail_Metric+1;
				  Write_Debug(Me,"Fail!!Tn Mdl"&Turnout'Img);
            end case;
            
         end if;
        
        if(Setting = Turned) then
            Polr_Opposite(Me) := Topolog2.Turnout_Data(Turnout).Swap_Pol_Tu;
            Write_Debug(Me,"Tn Blk Opp"&Polr_Opposite(Me)'Img);
        else
            Polr_Opposite(Me) := False;
        end if;        
         --Ada.Text_Io.Put_Line("Turnout info out"&Next_Block'Img&Converging'Img);--*
      end Get_Turning_Info;


      procedure Block_Acquire_Set (
            Fail_Metric:   in out Natural;
            Block    : in     Block_Idx;
            Polr     : in     Polarity_Type;
            -- TODO check potential fail of making Opp_Polr IN only (i.e. no reset to False)
            Opp_Polr : in     Boolean) is
               Success : Boolean;
      begin
        if(Block /= No_Block) then
            Write_Debug(Me,"Blk S"&Block'Img);
            --Ada.Text_Io.Put_Line("Acquire Set Block "&Block'Img);--*
            --TODO: Check 
            if(Blocks_Owned(Me,Block) /= True) then
                 Blocks.Acquire_Block(Success,Me,Block);  
    
                if(Success) then
                    --Ada.Text_IO.Put_Line("Through here no crash Block_Acquire_Set");
    
                    if ( Opp_Polr = False) then
                    Blocks.Set_Block(Success,Me,Block,Cab_Type(Me),Polr);
                    --Ada.Text_Io.Put_Line("Set Block "&Next_Block(I)'Img);
                    else
                    Blocks.Set_Block(Success,Me,Block,Cab_Type(Me),Opposite(Polr));
                    --Opp_Polr := False;
                    end if; 
                    if(Success = False) then
                        Fail_Metric:=Fail_Metric+1;
                        Write_Debug(Me,"Fail!!Set Block  "&Block'Img);
                        Blocks_Owned(Me,Block) := False;
                        --just add
                        Blocks.Release_Block(Success,Me,Block);
                    else
                        -- Succeeed!
                        Blocks_Owned(Me,Block) := True;
                    end if;
                    --Ada.Text_IO.Put_Line("Through herfe no crash Block_Acquire_Set");
                else
                    Fail_Metric:=Fail_Metric+1;
                    Write_Debug(Me,"Fail!!Ac Blk "&Block'Img);
                    Blocks_Owned(Me,Block) := False;
                end if;
            end if;
        else
            Fail_Metric:=Fail_Metric+1;
            Write_Debug(Me,"F! Emp B"&Block'Img);   
        end if;
        
      end Block_Acquire_Set;

      -- Thread check done
      procedure Turnout_Block_Set_Check (
            Fail_Metric      : in   out Natural;
            Block            :        Block_Idx;
            Polr             :        Polarity_Type;
            Turnout          :        Turnout_Id;
            Converging       :        Boolean;
            Required_Setting :        Turnout_Pos) is
            Setting :  Turnout_Pos;
			Success: Boolean;
      begin
        Setting := Required_Setting;
        
        -- Ada.Text_Io.Put_Line("Turnout Set Block "&Required_Setting'Img&Block'Img&Polr'Img&Polr_Opposite(Me)'Img);--*
        if(Converging) then
            if(Required_Setting /= Middle and Get_Position(Turnout) /= Required_Setting ) then
                -- Acquire Block prior to turning turnout
                Blocks.Acquire_Block(Success,Me,Topolog2.Turnout_Data(Turnout).Block_Num);
                if(Success) then
                    Set_Turnout(Turnout,Required_Setting);
                    Write_Debug(Me,"S Tn "&Turnout'Img&Required_Setting'Img); 
                else
                    Fail_Metric:=Fail_Metric+1;
                    Write_Debug(Me,"F! Tn Oc"&Turnout'Img);    
                end if;
            end if;
        else
           -- Only update required setting on Diverging turnout
            Setting := Get_Position(Turnout);
        end if;
        
        if(Setting = Turned) then
            Polr_Opposite(Me) := Topolog2.Turnout_Data(Turnout).Swap_Pol_Tu;
            Write_Debug(Me,"Tn Blk Opp"&Polr_Opposite(Me)'Img);
        else
            Polr_Opposite(Me) := False;
        end if;
        
         --Acquire and Set Block
         Block_Acquire_Set(Fail_Metric,Block,Polr,Polr_Opposite(Me));
		
		if(	Fail_Metric = 0) then 
			 -- Turn out in position?
             Write_Debug(Me,"G Tn "&Turnout'Img);
			 if(Turnout_Controller.Get_Status(Turnout) = True) then
                Write_Debug(Me,"Tn Done "&Turnout'Img);
				--Ada.Text_Io.Put_Line("Turnout Done Pos is "&Turnout_Controller.Get_Position(Turnout_Queue(Me))'Img);--*
				if (Turnout_Controller.Get_Position(Turnout) = Middle) then
                   --Set_Turnout(Turnout,Required_Setting);
				   Fail_Metric:=Fail_Metric+1;
				   Write_Debug(Me,"Fail!!Tn Mdl "&Turnout'Img);
				--else
				  -- Success := True; 
				end if;
			 else
				-- THREAD DEAD HERE!!!!
                --Set_Turnout(Turnout,Required_Setting);
				Fail_Metric:=Fail_Metric+1;
				Write_Debug(Me,"F! Tn /=Done "&Turnout'Img);
			 end if;
		end if;
      end Turnout_Block_Set_Check;
            
    
      procedure Turnout_Block_Acquire_Set (
            Fail_Metric          :  in  out Natural;
            Block            : in out    Block_Idx;
            Polr              : in      Polarity_Type;
            Which_Turnout    : in     Turnout_Id;
            Required_Setting : in Turnout_Pos -- for Converging turnout
            ) is
			Success: Boolean; 
        -- Want_Straight : Boolean; 
        Converging : Boolean;
      begin
        --Converging := False;
         ----Ada.Text_IO.Put_Line("Through here no crash Turnout_Block_Acquire_Set");--*
        -- Get_Turnout_Info(Fail_Metric,Block,Polr,Polr_Opposite(Me));
--         Converging := (Polr = Fwd and Topolog2.Turnout_Data(Which_Turnout).Diverging_When_Normal = False)
--                    or (Polr = Rev and Topolog2.Turnout_Data(Which_Turnout).Diverging_When_Normal) 
--                    or (Polr = Fwd and Topolog2.Turnout_Data(Which_Turnout).Swap_Pol_Tu 
--                                    and Topolog2.Turnout_Data(Which_Turnout).Diverging_When_Normal
--                                      and Block = Topolog2.Turnout_Data(Which_Turnout).Block_Num )  
--                    or (Polr = Rev and Topolog2.Turnout_Data(Which_Turnout).Swap_Pol_Tu 
--                            and Topolog2.Turnout_Data(Which_Turnout).Diverging_When_Normal=False);
                                 -- ensure actually converging
         Converging :=  Required_Setting /= Middle;        
        
         Get_Turning_Info(Fail_Metric,Block,Polr,Which_Turnout,Converging);

         ----Ada.Text_IO.Put_Line("Through here no crash Turnout_Block_Acquire_Set");
         Turnout_Block_Set_Check(Fail_Metric,Block,Trains_Pos(Me)(Front).Polr,Which_Turnout,Converging,Required_Setting);

      end Turnout_Block_Acquire_Set;


    
      function Check_Entering_Crossing_Extended (
         Pos      : in     Train_Position)
         return Feature_Descriptor_Extended is
         Entering :     Boolean;
         Which_Crossing    :  Crossing_Idx;
         Which_Crossing2    :  Crossing_Idx;
        
         Cr_Feat : Feature_Descriptor_Extended := Empty_Feature;
    begin
        Check_Entering_Crossing(Pos,Entering,Which_Crossing);    
        if Entering then 
            Cr_Feat := Feature_Descriptor_Extended'(Kind=>Crossing_Track,
                                            Block_Num=>No_Block,
                                            Block_Next => No_Block,
                                            Complete=>False,
                                            Kid => Which_Crossing,
                                            Kid2 => Which_Crossing
                                            );
            --Tram
            IF(Me = Tram_Id) THEN
                IF(Which_Crossing = North_Tram_Cross) THEN
                    Which_Crossing2 := South_Tram_Cross;
                END IF;
                IF(Which_Crossing = South_Tram_Cross) THEN
                    Which_Crossing2 := North_Tram_Cross;
                END IF;
            else
                Which_Crossing2 := Which_Crossing;
            END IF;
            
            --Extra Info
            Cr_Feat.Kid2 := Which_Crossing2;
            
            
            --Check
            Feature_Check_Halt(Cr_Feat);
        end if;
        
        return Cr_Feat;
        
    end Check_Entering_Crossing_Extended;
    
	
	function  Check_Entering_Zone_Extended (
         Pos      : in     Train_Position
         ) return Feature_Descriptor_Extended is
		 Entering :    Boolean;
         Which    :    Zone_Id;
         Z_Feat : Feature_Descriptor_Extended := Empty_Feature;
    begin
        Write_Debug(Me,"Ck En Z");
                Check_Entering_Zone(Pos,Entering,Which);
              if(Entering) then
                 Z_Feat := 
                    Feature_Descriptor_Extended'(Kind=>Boundary_Zone,
                                                Block_Num=>No_Block,
                                                Block_Next => No_Block,
                                                Complete=>False,
                                                Zid => Which
                                                );
                                               
                                               
                  Feature_Check_Halt(Z_Feat);                         
              end if;
          return Z_Feat;
             
	end   Check_Entering_Zone_Extended;
	
    	 
	 function  Check_Entering_Dead_Track (
		Pos     :  Train_Position  ) return   Feature_Descriptor_Extended
            is
        DT_Feat :  Feature_Descriptor_Extended := Empty_Feature;
	 begin
        
		if Is_Leaving_Powered_Track(Pos) then
            -- TODO check if has problem with MID-WAY THROUGH turning
            Actual_Halt(Manual=>False);
            DT_Feat := Feature_Descriptor_Extended'(Kind=>Dead_Track,
                                            Block_Num=>Pos.Block_Num,
                                            Block_Next => No_Block,
                                            Complete=>False
                                            );
            Feature_Check_Halt(DT_Feat); 
--            Turning_Around(Me) := True;
--			if(Pos.Polr = Fwd) then	
--				Start_Train_Backward(Me);
--			else
--				Start_Train_Forward(Me);
--			end if;
		end if;
        return DT_Feat;
	 end Check_Entering_Dead_Track;
	
    
    	 
      function Check_Entering_Turnout_Extended(
            Pos              : in     Train_Position  
            ) return Feature_Descriptor_Extended is
        Tn_Feat : Feature_Descriptor_Extended;
         Chained       : Chain_Type;
         Entering          : Boolean;
         Entering2         : Boolean;
         Which_Turnout     : Turnout_Idx := No_Turnout;
         Which_Turnout2    : Turnout_Idx := No_Turnout;
         Converging        : Boolean;
         Required_Setting  : Turnout_Pos := Middle;
         Converging2       : Boolean;    
         Required_Setting2 : Turnout_Pos := Middle;
         Fail_Metric : Natural := 0;
    begin
        Check_Entering_Turnout(Pos,Entering,Which_Turnout,Converging,Required_Setting, Chained);
        if Entering then
            --Create
            Tn_Feat := 
                Feature_Descriptor_Extended'(Kind=>Turnout,
                                            Block_Num=>No_Block,
                                            Block_Next => No_Block,
                                            Complete=>False,
                                            Tid => Which_Turnout,
                                            Setting =>Required_Setting,
                                            Block_Tn2 => No_Block,
                                            Next_Tu => 0,
                                            Setting2 => Middle,
                                            Tid2 => No_Turnout
                                            );
            --More Info
            Get_Turning_Info(Fail_Metric,Tn_Feat.Block_Num,Pos.Polr, Which_Turnout,Converging);
            
            --Chained
            if (Chained /= No and  Get_Position(Which_Turnout) /= Middle) then
                if(Required_Setting = Middle) then --diverge 
                    Required_Setting := Get_Position(Which_Turnout); 
                else
                    null; -- otherwise assumed above Required Pos
                end if;
                Check_Entering_Chained_Turnout(Pos,Required_Setting,Entering2,Which_Turnout2,Converging2,Required_Setting2);                      
                
                if (Entering2) then 
                    --Store
                    Tn_Feat.Tid2 := Which_Turnout2;
                    Tn_Feat.Setting2 := Required_Setting2;
                    --More Info 
                    Get_Turning_Info(Fail_Metric,Tn_Feat.Block_Tn2, Pos.Polr, Which_Turnout2,Converging2);
                    Write_Debug(Me,"Tn2S!"&Which_Turnout2'Img&Tn_Feat.Block_Tn2'Img);
                end if;
            end if;
            
            --Check -- if fail Halt
            Feature_Check_Halt(Tn_Feat);
        else
            Tn_Feat := Empty_Feature;
        end if; -- end Turnout Check
       
        return Tn_Feat;
    end Check_Entering_Turnout_Extended;

            
    procedure  Check_Leaving_Turnout_Extended (
            Pos        : in     Train_Position) is
         Chained       : Chain_Type;
         Leaving          : Boolean;
         Leaving2         : Boolean;
         Which_Turnout     : Turnout_Idx := No_Turnout;
         Which_Turnout2    : Turnout_Idx := No_Turnout;
         Converging        : Boolean;
         Required_Setting  : Turnout_Pos := Middle;
         Converging2       : Boolean;    
         Required_Setting2 : Turnout_Pos := Middle;
        Success : Boolean;
    begin

        --    Check_Leaving_Turnout 
        Check_Leaving_Turnout(Pos,Leaving,
            Which_Turnout,Converging,Chained);
        if Leaving then
            Write_Debug(Me,"Ck Lv Tn");
            Ada.Text_Io.Put_Line("Ck Lv Tn");--*
            --Release Turnout Block
            Release_Block_Extended(Success,Topolog2.Turnout_Data(Which_Turnout).Block_St);
            Release_Block_Extended(Success,Topolog2.Turnout_Data(Which_Turnout).Block_Tu);
    --        Blocks.Set_Block(Success,Me,Topolog2.Turnout_Data(Which_Turnout).Block_Num,0,Fwd);
    --        if(Success = False) then
    --            Write_Debug(Me,"F! Rl Blk Lv Tn"&Which_Turnout'Img);
    --        else      
    --            Blocks.Release_Block(Success,Me,Topolog2.Turnout_Data(Which_Turnout).Block_Num);                
    --            Write_Debug(Me, "S! Rl Blk "&Topolog2.Turnout_Data(Which_Turnout).Block_Num'Img);
    --        end if;
            if (Chained = Immediate) then
                Check_Leaving_Chained_Turnout(Pos,Get_Position(Which_Turnout),
                    Leaving2,Which_Turnout2,Converging2);
                Release_Block_Extended(Success,Topolog2.Turnout_Data(Which_Turnout2).Block_St);
                Release_Block_Extended(Success,Topolog2.Turnout_Data(Which_Turnout2).Block_Tu);
                Release_Block_Extended(Success,Topolog2.Turnout_Data(Which_Turnout2).Block_Num);
--                Blocks.Set_Block(Success,Me,Topolog2.Turnout_Data(Which_Turnout2).Block_Num,0,Fwd);                
--                if(Success = False) then
--                    Write_Debug(Me,"F! Rl Blk Lv Tn"&Which_Turnout2'Img);
--                else      
--                    Blocks.Release_Block(Success,Me,Topolog2.Turnout_Data(Which_Turnout2).Block_Num);  
--                    Write_Debug(Me, "S! Rl Blk "&Topolog2.Turnout_Data(Which_Turnout2).Block_Num'Img);    
--                end if; 
            end if;
        end if; 
      
        
    end Check_Leaving_Turnout_Extended ;
    
    procedure  Check_Leaving_Crossing_Extended (
            Pos        : in     Train_Position) is
         Which_Crossing  : Crossing_Idx;
         Which_Crossing2 : Crossing_Idx;
         Leaving         : Boolean;
    begin
        Write_Debug(Me,"Ck Lv Cr");
        -- Crossing
        Check_Leaving_Crossing(Pos,Leaving,Which_Crossing);
        if(Leaving) then
            Release_Crossing(Me,Which_Crossing);
            Write_Debug(Me,"Lv Cro"&Which_Crossing'Img);
            
            if(Me = Tram_Id) then
                if(Which_Crossing = North_Tram_Cross) then
                    Which_Crossing2 := South_Tram_Cross;
                end if;
                
                if(Which_Crossing = South_Tram_Cross) then
                    Which_Crossing2 := North_Tram_Cross;
                end if;
        
                Release_Crossing(Me,Which_Crossing2);
                Write_Debug(Me,"Lv Cro"&Which_Crossing2'Img);
            end if;
            
        end if;

    end Check_Leaving_Crossing_Extended ;
    
    procedure  Check_Leaving_Zone_Extended (
            Pos        : in     Train_Position) is
        Which_Zone      : Zone_Id;
        Leaving : Boolean;
    begin
        Write_Debug(Me,"Ck Lv Z");
        Check_Leaving_Zone(Pos,Leaving,Which_Zone);
        if (Leaving) then
            Release_Zone(Me,Which_Zone);
            Write_Debug(Me, "Lv Zone "&Which_Zone'Img);
        end if;
    end Check_Leaving_Zone_Extended ;
    
    
	procedure Advance_Pos_Extended(
			Which_End: Position_Ref;
            State: Sensor_Bit
            ) is
        Need_Setting  : Boolean;
        For_Turnout   : Turnout_Id;
        Topolog_Stopping : Boolean;
        Other_End: Position_Ref;
    begin
        if(State /= Prev_S_Evt_State(Me,Which_End)) then
            Prev_S_Evt_State(Me,Which_End)  := State;

            if Which_End /= Front then
                Topolog_Stopping := False;
    
            else
                Topolog_Stopping := Stopping(Me);
            end if;
            --Ada.Text_Io.Put_Line("Advance Pos for "&Which_End'Img&Topolog_Stopping'Img);
            Topolog2.Advance_Pos(Trains_Pos(Me)(Which_End),Topolog_Stopping,Need_Setting,For_Turnout);
            if(Need_Setting) then --need turn out position -- mean Diverge
                -- Current Turnout position in used
                --Ada.Text_Io.Put_Line("Need Setting Position"&Get_Position(For_Turnout)'Img);
                Topolog2.Advance_Pos(Trains_Pos(Me)(Which_End),Topolog_Stopping,
                    Get_Position(For_Turnout));
            end if;
                
        else
            -- NOT SUPPOSED TO BE!
            Write_Debug(Me,"F!SEvt State");
            --Actual_Halt(Manual => True);    
            
            -- Other instead
          --  if(Which_End = Front) then Other_End := Rear; end if;
          --  if(Which_End = 
            null;
        end if;
                            
                
    end Advance_Pos_Extended;
	
    procedure Update_Feature_Queue is
        
    begin
        ---------------------------------------------------------
        -- PLAIN TRACK
        ---------------------------------------------------------       
        Feature_Queue(Me,Plain_Track) := Empty_Feature;
        Feature_Queue(Me,Plain_Track).Block_Num := Topolog2.Sensor_Block_Numbers(
                            Trains_Pos(Me)(Front).Polr,Trains_Pos(Me)(Front).Next_Expected);
        Feature_Queue(Me,Plain_Track).Complete := False;
        
        ---------------------------------------------------------
        -- TURN OUT
        ---------------------------------------------------------       
        Feature_Queue(Me,Turnout) := Check_Entering_Turnout_Extended(Trains_Pos(Me)(Front));
        
        ---------------------------------------------------------
        -- CROSSING ---------------------------------------------------------             
        Feature_Queue(Me,Crossing_Track) := Check_Entering_Crossing_Extended(Trains_Pos(Me)(Front));
        
        ---------------------------------------------------------
        -- Zone ---------------------------------------------------------          
        Feature_Queue(Me,Boundary_Zone) := Check_Entering_Zone_Extended(Trains_Pos(Me)(Front));
    
        ---------------------------------------------------------
            -- Powered Track
        ---------------------------------------------------------  
        Feature_Queue(Me,Dead_Track) := Check_Entering_Dead_Track(Trains_Pos(Me)(Front));
    end Update_Feature_Queue;
            
    procedure Sensor_Event_Full(
            S_Id      : in     Raildefs.Sensor_Id;
            S_State   : in     Raildefs.Sensor_Bit;
            Which_End : in     Position_Ref
            ) is
           Old_Block       : Block_Idx;
           Block_To_Be_Released : Block_Idx;
           Success : Boolean;
           --Which_End : Position_Ref; 
    begin
        Old_Block := Trains_Pos(Me)(Which_End).Block_Num;
         case Which_End is 
            when Front=>
                --Sensor Event should happen only if Feature Queue is EMPTY
                Ada.Text_Io.Put_Line("SE"&Which_End'Img&S_Id'Img&S_State'Img); --*
                for F in Feature_Kind loop
                    if(Feature_Queue(Me,F) /= Empty_Feature) then
                        Write_Debug(Me,"F/=Emp SEvt"&F'Img);
                        Actual_Halt(Manual => False); --software halt
                    end if;
                end loop;  
            
                -- Advance Pos 
                Advance_Pos_Extended(Front,S_State);    
                
                --   Write_Debug(Me,"S Evt"&S_State'Img&Fail_Metric'Img);
                if(S_State = On) then
                    --Speed back
                    Check_Speed_Back;     
                    Update_Feature_Queue;
--                else
--                    -- Some safety Caution
--                    Check_Go_Slow;
                end if; 
            when Rear =>
                Ada.Text_Io.Put_Line("SE"&Which_End'Img&S_Id'Img&S_State'Img); --*
               if (S_State = Off) then -- OFF state Checks

                ---------------------------------------------------------
                -- TURN OUT
                ---------------------------------------------------------       
                Check_Leaving_Turnout_Extended(Trains_Pos(Me)(Rear));
                Check_Leaving_Turnout_Extended(Trains_Pos(Me)(Tail_Release));
                
                ---------------------------------------------------------
                -- CROSSING ---------------------------------------------------------             
                Check_Leaving_Crossing_Extended(Trains_Pos(Me)(Rear));
                Check_Leaving_Turnout_Extended(Trains_Pos(Me)(Tail_Release));
                
                ---------------------------------------------------------
                -- Zone ---------------------------------------------------------          
                Check_Leaving_Zone_Extended(Trains_Pos(Me)(Rear));
                Check_Leaving_Turnout_Extended(Trains_Pos(Me)(Tail_Release));
               end if; -- END OFF STATE CHECK
   
                ---------------------------------------------------------                
                --Implemetation of Tail_Release
                ---------------------------------------------------------                               
                
               Advance_Pos_Extended(Rear,S_State); 
                if (Tail_Release_Delay(Me) = 0) then
                    Advance_Pos_Extended(Tail_Release,S_State);
                    Block_To_Be_Released := Trains_Pos(Me)(Tail_Release).Block_Num ;
                      if(Block_To_Be_Released /= Topolog2.Sensor_Block_Numbers(Trains_Pos(Me)(Rear).Polr,S_Id)) then
                        Release_Block_Extended(Success,Block_To_Be_Released);
                      end if;    

               else
                  Tail_Release_Delay(Me)  := Tail_Release_Delay(Me)  - 1;     
               end if;
                
            when Tail_Release =>
               null;
         end case;
 
        -- Debugging
       Put_Line(Debug_W(Me),Trains_Pos(Me)(Front).Block_Num'Img&Trains_Pos(Me)(Front).Next_Expected'Img
              &Trains_Pos(Me)(Rear).Block_Num'Img&Trains_Pos(Me)(Rear).Next_Expected'Img
            &Trains_Pos(Me)(Tail_Release).Block_Num'Img&Trains_Pos(Me)(Tail_Release).Next_Expected'Img,
            1,Foreground=>Swindows.Green);
        Ada.Text_Io.Put_Line(Trains_Pos(Me)(Front).Block_Num'Img&Trains_Pos(Me)(Front).Next_Expected'Img--*
              &Trains_Pos(Me)(Rear).Block_Num'Img&Trains_Pos(Me)(Rear).Next_Expected'Img--*
            &Trains_Pos(Me)(Tail_Release).Block_Num'Img&Trains_Pos(Me)(Tail_Release).Next_Expected'Img);--*
    end Sensor_Event_Full;
    
   
      procedure Actual_Sensor_Event (
            S_Id      : in     Raildefs.Sensor_Id;
            S_State   : in     Raildefs.Sensor_Bit
            ) is
           Which_End : Position_Ref; 
      begin
        if(Trains_Pos(Me)(Front).Next_Expected = S_Id) then
            Which_End := Front;
        else
            if Trains_Pos(Me)(Rear).Next_Expected = S_Id then
                Which_End := Rear;    
            else
                Which_End := Tail_Release; -- Skipp ALL operation! NOT ME
            end if;         
        end if;
		 
         --Full sensor event
         Sensor_Event_Full(S_Id,S_State,Which_End);
           
      end Actual_Sensor_Event;



      procedure Actual_Tick is
         Resume_Success : Boolean := False;
         Fail_Metric : Natural := 0;
      begin
         --Ada.Text_Io.Put_Line("Tick"&Feature_Queue(Me).Complete'Img &Feature_Queue(Me).Block_Num'Img );

        --if(Tick_Delay = 0) then
    		for I in Feature_Kind loop
    		
             if(Feature_Queue(Me,I).Complete = False or Feature_Queue(Me,I).Block_Num /= No_Block) then -- Something need to be done
                Write_Debug(Me,"Tk K "&Feature_Queue(Me,I).Kind'Img&"F "&Fail_Metric'Img);
                Feature_Check(Fail_Metric,Feature_Queue(Me,I));
                --Feature_Check_Halt(Feature_Queue(Me,I));
             end if;

    		end loop;

            if( Debug_W_Delay = 0) then	
                if (Fail_Metric /= 0) then
                    Write_Debug(Me,"Tk "&Fail_Metric'Img);
                    null;
                end if; 
            else
                Debug_W_Delay := Debug_W_Delay -1;
            end if;

    		End_Safe_Check(Fail_Metric = 0);

--        else
--            Tick_Delay := Tick_Delay -1;
--        end if;

      end Actual_Tick;
    

      --Forward
      procedure Actual_Forward is
      begin
        if (T_Mode_Manned(Me)) then
             if( Trains_Pos(Me)(Front).Polr /= Fwd) then
                Feature_Queue(Me,Dead_Track) := Feature_Descriptor_Extended'(Kind=>Dead_Track,
                    Block_Num=> Trains_Pos(Me)(Front).Block_Num,
                    Block_Next => No_Block,
                    Complete=>False
                    );
                Feature_Check_Halt(Feature_Queue(Me,Dead_Track));
            end if;
        end if;
        
      end Actual_Forward;

      --Backward
      procedure Actual_Backward is

      begin
        if (T_Mode_Manned(Me)) then
         if( Trains_Pos(Me)(Front).Polr /= Rev) then
            --Ada.Text_Io.Put_Line("BWD HERE");
            Feature_Queue(Me,Dead_Track) := Feature_Descriptor_Extended'(Kind=>Dead_Track,
                Block_Num=>Trains_Pos(Me)(Front).Block_Num,
                Block_Next => No_Block,
                Complete=>False
                );
            Feature_Check_Halt(Feature_Queue(Me,Dead_Track));
         end if;
        end if;
      end Actual_Backward;

    
    --Turn Left
      procedure Actual_Left (
            T_Id           : in     Raildefs.Train_Id;
            Position_Front : in     Topolog2.Train_Position) is

         Next_Sensor : Raildefs.Sensor_Idx;
         Turnout_No  : Raildefs.Turnout_Idx        := 0;
         Block_No    : Raildefs.Block_Id;
         Turnout     : Raildefs.Turnout_Idx;
         Test        : Topolog2.Feature_Descriptor;
         Kind        : Topolog2.Feature_Kind;


      begin

         if (T_Mode_Manned(Me)) then
            --just add
            Actual_Halt(Manual=>False);
            Block_No:=Position_Front.Block_Num;

            if  Position_Front.Polr = Fwd then --If going CCW

               for I in Blocks_Fwd'range loop
                  if Block_No=Bl_Fwd(I) then
                     Turnout :=Tn_Fwd(I);
                     --work out the next turnout
                     --if we can turn left turn left if not exit
                     Test:=Topolog2.Feature_At(Position_Front);

                     if Test.Kind=Topolog2.Turnout then
                        case Test.Kind is
                           when Topolog2.Turnout=>
                              if Test.Tid=Turnout then
                                 Ada.Text_Io.Put_Line("Test id: "&Test.Tid 'Img);--*
                                 return;
                              else
                                 if Topolog2.Turnout_Data(Turnout).
                                       Straight_Is_Left then
                                    Turnout_Controller.Set_Turnout(
                                       Turnout ,Straight);
                                 else
                                    --if not turn turnout
                                    Turnout_Controller.Set_Turnout(
                                       Turnout ,Turned);
                                 end if;
                              end if;
                           when others=>
                              null;
                        end case;
                     else
                        if Topolog2.Turnout_Data(Turnout).
                              Straight_Is_Left then
                           Turnout_Controller.Set_Turnout(Turnout ,
                              Straight);
                        else
                           --if not turn turnout
                           Turnout_Controller.Set_Turnout(Turnout ,Turned);
                        end if;
                     end if;
                  end if;
               end loop;
            else
               for I in Blocks_Rev'range loop
                  if Block_No=Bl_Rev(I) then
                     Turnout :=Tn_Rev(I);
                     --work out the next turnout
                     --if we can turn left turn left if not exit
                     Test:=Topolog2.Feature_At(Position_Front);
                     if Test.Kind=Topolog2.Turnout then
                        case Test.Kind is
                           when Topolog2.Turnout=>
                              if Test.Tid=Turnout then
                                 return;
                              else
                                 if Topolog2.Turnout_Data(Turnout).
                                       Straight_Is_Left then
                                    Turnout_Controller.Set_Turnout(
                                       Turnout ,Straight);
                                 else
                                    --if not turn turnout
                                    Turnout_Controller.Set_Turnout(
                                       Turnout ,Turned);
                                 end if;
                              end if;
                           when others=>
                              null;
                        end case;
                     else
                        if Topolog2.Turnout_Data(Turnout).
                              Straight_Is_Left then
                           Turnout_Controller.Set_Turnout(Turnout ,
                              Straight);
                        else
                           --if not turn turnout
                           Turnout_Controller.Set_Turnout(Turnout ,Turned);
                        end if;
                     end if;
                  end if;
               end loop;
            end if;
         end if;
      end Actual_Left;


      procedure Actual_Right (
            T_Id           : in     Raildefs.Train_Id;
            Position_Front : in     Topolog2.Train_Position) is

         Next_Sensor : Raildefs.Sensor_Idx;
         Turnout_No  : Raildefs.Turnout_Idx        := 0;
         Block_No    : Raildefs.Block_Id;
         Turnout     : Raildefs.Turnout_Idx;
         Test        : Topolog2.Feature_Descriptor;
         Kind        : Topolog2.Feature_Kind;



      begin
         if (T_Mode_Manned(Me)) then
            --just add
            Actual_Halt(Manual=>False);
            Block_No:=Position_Front.Block_Num;

            if  Position_Front.Polr = Fwd then --If going CCW

               for I in Blocks_Fwd'range loop
                  if Block_No=Bl_Fwd(I) then
                     Turnout :=Tn_Fwd(I);
                     --work out the next turnout
                     --if we can turn left turn left if not exit
                     Test:=Topolog2.Feature_At(Position_Front);
                     if Test.Kind=Topolog2.Turnout then
                        case Test.Kind is
                           when Topolog2.Turnout=>
                              if Test.Tid=Turnout then
                                 Ada.Text_Io.Put_Line("Test id: "&Test.Tid 'Img);--*
                                 return;
                              else
                                 if Topolog2.Turnout_Data(Turnout).
                                       Straight_Is_Left then
                                    Turnout_Controller.Set_Turnout(
                                       Turnout ,Turned);
                                 else
                                    --if not turn turnout
                                    Turnout_Controller.Set_Turnout(
                                       Turnout ,Straight);
                                 end if;
                              end if;
                           when others=>
                              null;
                        end case;
                     else
                        if Topolog2.Turnout_Data(Turnout).
                              Straight_Is_Left then
                           Turnout_Controller.Set_Turnout(Turnout ,
                              Turned);
                        else
                           --if not turn turnout
                           Turnout_Controller.Set_Turnout(Turnout ,
                              Straight);
                        end if;
                     end if;
                  end if;
               end loop;
            else
               for I in Blocks_Rev'range loop
                  if Block_No=Bl_Rev(I) then
                     Turnout :=Tn_Rev(I);
                     --work out the next turnout
                     --if we can turn left turn left if not exit
                     Test:=Topolog2.Feature_At(Position_Front);
                     if Test.Kind=Topolog2.Turnout then
                        case Test.Kind is
                           when Topolog2.Turnout=>
                              if Test.Tid=Turnout then
                                 return;
                              else
                                 if Topolog2.Turnout_Data(Turnout).
                                       Straight_Is_Left then
                                    Turnout_Controller.Set_Turnout(
                                       Turnout ,Turned);
                                 else
                                    --if not turn turnout
                                    Turnout_Controller.Set_Turnout(
                                       Turnout ,Straight);
                                 end if;
                              end if;
                           when others=>
                              null;
                        end case;
                     else
                        if Topolog2.Turnout_Data(Turnout).
                              Straight_Is_Left then
                           Turnout_Controller.Set_Turnout(Turnout ,
                              Turned);
                        else
                           --if not turn turnout
                           Turnout_Controller.Set_Turnout(Turnout ,
                              Straight);
                        end if;
                     end if;
                  end if;
               end loop;

               Next_Sensor:= Position_Front.Next_Expected;
               if Block_No=5 then
                  if  (Next_Sensor=26) then
                     Turnout := 5;
                     Turnout_Controller.Set_Turnout(Turnout ,Turned);
                  else
                     if  (Next_Sensor=16) then
                        Turnout := 4;
                        Turnout_Controller.Set_Turnout(Turnout ,Turned);
                     else
                        if (Next_Sensor=14) then
                           Turnout := 3;
                           Turnout_Controller.Set_Turnout(Turnout ,
                              Turned);
                        else
                           if (Next_Sensor=12) then
                              Turnout := 2;
                              Turnout_Controller.Set_Turnout(Turnout ,
                                 Turned);
                           end if;
                        end if;
                     end if;
                  end if;
               end if;
            end if;
         end if;

      end Actual_Right;





      procedure Actual_Centre (
            T_Id           : in     Raildefs.Train_Id;
            Position_Front : in     Topolog2.Train_Position) is

         Next_Sensor : Raildefs.Sensor_Idx;
         Turnout_No  : Raildefs.Turnout_Idx        := 0;
         Block_No    : Raildefs.Block_Id;
         Turnout     : Raildefs.Turnout_Idx;
         Test        : Topolog2.Feature_Descriptor;
         Kind        : Topolog2.Feature_Kind;


      begin

         if (T_Mode_Manned(Me)) then
            --just add
            Actual_Halt(Manual=>False);
            Block_No:=Position_Front.Block_Num;



            if  Position_Front.Polr = Fwd then --If going CCW

               for I in Blocks_Fwd'range loop
                  if Block_No=Bl_Fwd(I) then
                     Turnout :=Tn_Fwd(I);
                     --work out the next turnout
                     --if we can turn left turn left if not exit
                     Test:=Topolog2.Feature_At(Position_Front);
                     if Test.Kind=Topolog2.Turnout then
                        case Test.Kind is
                           when Topolog2.Turnout=>
                              if Test.Tid=Turnout then
                                 return;
                              else
                                 Turnout_Controller.Set_Turnout(Turnout ,Straight);
                              end if;
                           when others=>
                              null;
                        end case;
                     else
                        Turnout_Controller.Set_Turnout(Turnout ,Straight);
                     end if;
                  end if;
               end loop;
            else
               for I in Blocks_Rev'range loop
                  if Block_No=Bl_Rev(I) then
                     Turnout :=Tn_Rev(I);
                     --work out the next turnout
                     --if we can turn left turn left if not exit
                     Test:=Topolog2.Feature_At(Position_Front);
                     if Test.Kind=Topolog2.Turnout then
                        case Test.Kind is
                           when Topolog2.Turnout=>
                              if Test.Tid=Turnout then
                                 return;
                              else
                                 Turnout_Controller.Set_Turnout(Turnout ,Straight);
                              end if;
                           when others=>
                              null;
                        end case;
                     else
                        Turnout_Controller.Set_Turnout(Turnout ,Straight);
                     end if;
                  end if;
               end loop;
               Next_Sensor:= Position_Front.Next_Expected;
               if Block_No=5 then
                  if  (Next_Sensor=26) then
                     Turnout := 5;
                     Turnout_Controller.Set_Turnout(Turnout ,Straight);
                  else
                     if  (Next_Sensor=16) then
                        Turnout := 4;
                        Turnout_Controller.Set_Turnout(Turnout ,Straight);
                     else
                        if (Next_Sensor=14) then
                           Turnout := 3;
                           Turnout_Controller.Set_Turnout(Turnout ,Straight);
                        else
                           if (Next_Sensor=12) then
                              Turnout := 2;
                              Turnout_Controller.Set_Turnout(Turnout ,Straight);
                           end if;
                        end if;
                     end if;
                  end if;
               end if;
            end if;
         end if;
      end Actual_Centre; 

    
      --Speed controller
      procedure Actual_Mode (
            Mode : in     Mode_Ops) is
      begin
         case Mode is
            when Manned=>
               T_Mode_Manned(Me):=True;
            when Unmanned=>
               T_Mode_Manned(Me):=False;


         end case;



      end Actual_Mode;


 

      Item : Item_Type;
      Oops : Boolean;
   begin
        --SPEED SLOW
        SPEED_SLOW      := 170; -- real
        SPEED_SLOW_T1   := 220; -- real
        SPEED_SLOW      := 220; --simulation --*
        SPEED_SLOW_T1   := 220; --simulation --*
      loop

         Buffers(Me).Wait_Start(Item, Over_Run=>Oops);
         -- Check init of Feature Queue is fine
         ---Ada.Text_Io.Put("Feature "&Feature_Queue(Me).Kind'Img);
         --Ada.Text_Io.Put(Me'Img & " Req=" & Item.Req'Img);  --debug
         case Item.Req is
            -- when Dac => -- or case statement
            -- Actual_Dac(Item.D_Id,Item.Value);
            -- when Block =>
            -- Actual_Block(Item.B_Id,Item.B_Pol,Item.B_Cab);
            when Forward=>
               Actual_Forward ;
            when Backward=>
               Actual_Backward;
            when Left=>
               Actual_Left (Item.L_Id,Trains_Pos(Item.L_Id)(Front));
            when Right=>
               Actual_Right (Item.R_Id,Trains_Pos(Item.R_Id)(Front));
            when Centre=>
               Actual_Centre (Item.C_Id, Trains_Pos(Item.C_Id)(Front));
            when Speed =>
               Actual_Speed_Set(Item.Speed_Value);
            when Fast =>
               --Ada.Text_IO.Put_Line("TRAM HERE");
               Actual_Speed_Op(Item.Req);
            when Slow =>
               Actual_Speed_Op(Item.Req);
            when Halt =>
               Actual_Halt;
            when Sensor_Event=>
               Actual_Sensor_Event(Item.S_Id,Item.S_State);
            when Tick =>
               Actual_Tick;
            when Manned=>
               Actual_Mode(Item.Req);

            when Unmanned=>
               Actual_Mode(Item.Req);
         end case;
         if(Oops = True) then
           Write_Debug(Me," Over_Run=" & Oops'Img);   
         end if;
         
         --delay 1.0;
         -- NB the above delay statement is for test/demo ONLY.
         -- Normal sporadics do NOT have delays in their bodies.
      end loop;
   end Thread_Type;

    
    
  
end Controllers;